#ifndef RIKIBOT_DRIVER_HPP
#define RIKIBOT_DRIVER_HPP

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <serial/serial.h>
#include <cmath>
#include <cstring>

#define START_FRAME 0xABCD

#pragma pack(push, 1)
typedef struct {
    uint16_t start;
    int16_t rpmR;
    int16_t rpmL;
    int16_t batVoltage;
    int16_t boardTemp;
    int16_t curL_DC;
    int16_t curR_DC;
    uint16_t checksum;
} RikibotFeedback;

typedef struct {
    uint16_t start;
    int16_t mSpeedR;
    int16_t mSpeedL;
    uint16_t checksum;
} RikibotCommand;
#pragma pack(pop)

class RikibotDriver : public rclcpp::Node {
public:
    RikibotDriver();
    void run();

private:
    void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
    void readSerial();
    void processFeedback(const RikibotFeedback &fb);
    uint16_t calculateChecksum(const RikibotFeedback &fb);
    uint16_t calculateCommandChecksum(const RikibotCommand &cmd);
    uint16_t ntohs(uint16_t value);

    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
    rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    serial::Serial serial_;
    std::string port_;
    int baudrate_;
    std::string base_frame_id_;
    std::string odom_frame_id_;
    bool publish_tf_;
    bool swap_bytes_;

    // 调整顺序以匹配初始化列表
    double x_;
    double y_;
    double theta_;
    
    double wheel_diameter_;
    double wheel_distance_;
    double wheel_circumference_;

    float battery_full_voltage;
    float battery_empty_voltage;

    rclcpp::Time last_time_;
    
    // 串口缓冲区
    uint8_t serial_buffer_[256];
    size_t buffer_pos_;
};

#endif // RIKIBOT_DRIVER_HPP